#include <avr/io.h>
#include <avr/cpufunc.h>
#include <avr/pgmspace.h>
#include <stdio.h>
#include "Common.h"
#include "tests.h"

bool doVoltageTest() {
	bool good = true;
	
	printf_P( PSTR( "-5V\n" ) );
	uint32_t m5 = analogRead( A13 ); m5 = 1023 - m5; m5 = (m5 << 22) / 4294967;
	printf_P( PSTR( "+5V\n" ) );
	uint32_t p5 = analogRead( A14 ); p5 = (p5 << 22) / 4294967;
	printf_P( PSTR( "+12V\n" ) );
	uint32_t p12 = analogRead( A15 ); p12 = (p12 << 22) / 1789569;
	
	printf_P( PSTR( "Voltages: -%d.%02d %d.%02d %d.%02d\n" ),
	(uint16_t)(m5/100), (uint16_t)(m5 % 100),
	(uint16_t)(p5/100), (uint16_t)(p5 % 100),
	(uint16_t)(p12/100), (uint16_t)(p12 % 100));

	if( m5 < 475 || m5 > 525 ) {
		printf_P( PSTR( "-5V out of tolerance\n" ) );
		good = false;
	}
	if( p5 < 475 || p5 > 525 ) {
		printf_P( PSTR( "+5V out of tolerance\n" ) );
		good = false;
	}
	if( p12 < 1140 || p12 > 1260 ) {
		printf_P( PSTR( "+12V out of tolerance\n" ) );
		good = false;
	}

	return good;
}

bool doBusAck() {
	// Pull up MREQ, IORQ, RD, and WR before tri-stating the bus
	DDRH &= 0b11101000;
	PORTH |= 0b00010111;
	
	// See if BUSACK is already asserted.  If so, did we do it?
	// And if that's the case, then good job, return.
	if( ( PINE & 0b01000000 ) == 0 ) // BUSACK
	{
		if( ( DDRH & 0b00001000 ) != 0 && // BUSRQ
		( PORTH & 0b00001000 ) == 0 ) {
			return true;
		}
		printf_P( PSTR( "BUSACK asserted even though not requested.  fail\n" ) );
		return false;
	}

	printf_P( PSTR( "BUSACK " ) );

	// Pull BUSRQ low
	DDRH |= 0b00001000;
	PORTH &= 0b11110111;

	// Wait for BUSACK
	uint16_t count;
	for( count = 0; count < 10000; count++ )
	{
		if( ( PINE & 0b01000000 ) == 0 ) // BUSACK
		{
			break;
		}
	}

	if( count < 10000 )
	{
		printf_P( PSTR( "pass\n" ) );
	}
	else
	{
		printf_P( PSTR( "fail\n" ) );
		DDRH &= 0b11110111;
		PORTH &= 0b11110111;
		return false;
	}

	return true;
}

static bool doClockTest()
{
	DDRG &= 0b11111011; // Clock lines are inputs
	PORTG &= 0b11111011; // Make sure they aren't pulling up

	DDRJ &= 0b11011111;
	PORTJ &= 0b11011111;
	
	#define SAMPLE_CNT 250

	// Start by grabbing a bunch of samples.
	uint8_t samples[SAMPLE_CNT];
	for( uint8_t i = 0; i < SAMPLE_CNT; i++ )
	{
		samples[i] = (PING & 0b00000100) | (PINJ & 0b00100000);
	}

	// See if the there is a similar sample between uptime and downtime.  We don't have enough clock cycles to perform a real frequency test, but
	// it is at least possible to see if the clock is ticking.  It is assumed, of course, that the clock is different enough from the Atmel's to
	// get a near 50% up/down cycle, even with aliasing errors.
	//
	// Note 2:  CLK and CLK2 are electrically the same, save one resistor.  So perhaps it's really not necessary to check both of them.  Eh.

	int clk0h = 0;
	int clk0l = 0;
	int clk1h = 0;
	int clk1l = 0;
	for( uint8_t i = 1; i < SAMPLE_CNT; i++ )
	{
		(samples[i] & 0b00100000) ? clk1h++ : clk1l++;
		(samples[i] & 0b00000100) ? clk0h++ : clk0l++;
	}

	printf_P( PSTR(
	"CLK0: %d:%d\n"
	"CLK1: %d:%d\n" )
	, clk0h, clk0l
	, clk1h, clk1l
	);

	bool pass = true;

	printf_P( PSTR( "CLK0 " ) );
	if( clk0h > 20 && clk0l > 20 )
	{
		printf_P( PSTR( "seems ok\n" ) );
	}
	else
	{
		pass = false;
		printf_P( PSTR( "fail\n" ) );
	}

	printf_P( PSTR( "CLK1 " ) );
	if( clk1h > 20 && clk1l > 20 )
	{
		printf_P( PSTR( "seems ok\n" ) );
	}
	else
	{
		pass = false;
		printf_P( PSTR( "fail\n" ) );
	}

	return pass;
}

static bool doResetTest() {
	bool pass = true;

	DDRJ &= 0b01101111;

	// First check that RESET is in the correct resting state
	uint8_t s = PINJ;
	if( ( s & 0b00010000 ) != 0 ) { // RESET
		printf_P( PSTR( "RESET is stuck active\n" ) );
		pass = false;
	}

	if( ( s & 0b10000000 ) == 0 ) { // #RESET
		printf_P( PSTR( "#RESET is stuck active\n" ) );
		if( ( s & 0b00010000 ) == 0 ) { // RESET
			printf_P( PSTR( "RESET is okay.  Therefore, likely problem with U7\n" ) );
		}
		pass = false;
	}

	// See if external reset works
	DDRG |= 0b00000001;
	PORTG &= 0b11111110;

	uint32_t count;
	for( count = 0; count < 10000 && ( PINJ & 0b00010000 ) == 0; count++ ) { }

	s = PINJ;
	if( ( s & 0b00010000 ) == 0 ) { // RESET
		printf_P( PSTR( "RESET did not respond to signal\n" ) );
		pass = false;
	}

	if( ( s & 0b10000000 ) != 0 ) { // #RESET
		printf_P( PSTR( "#RESET did not respond to signal\n" ) );
		if( ( s & 0b00010000 ) != 0 ) { // RESET
			printf_P( PSTR( "RESET is okay.  Therefore, likely problem with U7\n" ) );
		}
		pass = false;
	}

    if( pass )
	{
		delay( 1000 );
	}
	
	DDRG &= 0b11111110;
	PORTG &= 0b11111110;

	// If the reset circuit is broken, don't attempt to test any further
	if( !pass ) {
		return pass;
	}
	
	// Count the time it takes reset to fall inactive again.  Should be a few milliseconds
	for( count = 0; count < 100000 && ( PINJ & 0b00010000 ) != 0; count++ ) { }

	printf_P( PSTR( "RESET recovery time: %lu\n" ), (long unsigned int) count );
	if( count < 60 ) {
		printf_P( PSTR( "Check RESET capacitor\n" ) );
		pass = false;
	}
	if( count > 100000 )
	{
		printf_P( PSTR( "RESET never recovered.  Check reset pull-up resistor and U7\n" ) );
		pass = false;
	}

	delay( 100 ); // Give CPU time to wake up

	return pass;
}


static void printStuckPins( uint8_t data, uint8_t addrlow, uint8_t addrhigh, uint8_t buslines )
{
	if( data != 0 || addrlow != 0 || addrhigh != 0 || buslines != 0)
	{
		printf_P( PSTR( "Failed bus lines:") );
	}

	for( uint8_t ct = 0; data != 0; data >>= 1, ct++ )
	{
		if( ( data & 1 ) != 0 )
		{
			printf_P( PSTR( " D%u" ), ct );
		}
	}
	
	for( uint8_t ct = 0; addrlow != 0; addrlow >>= 1, ct++ )
	{
		if( ( addrlow & 1 ) != 0 )
		{
			printf_P( PSTR( " A%u" ), ct );
		}
	}
	
	for( uint8_t ct = 8; addrhigh != 0; addrhigh >>= 1, ct++ )
	{
		if( ( addrhigh & 1 ) != 0 )
		{
			printf_P( PSTR( " A%u" ), ct );
		}
	}

	for( uint8_t ct = 0; buslines != 0; buslines >>= 1, ct++ )
	{
		if( ( buslines & 1 ) != 0 )
		{
			static char const l0[] PROGMEM = " WR";
			static char const l1[] PROGMEM = " RD";
			static char const l2[] PROGMEM = " MREQ";
			static char const l3[] PROGMEM = " (3)";
            static char const l4[] PROGMEM = " IORQ";
            static char const l5[] PROGMEM = " (5)";
            static char const l6[] PROGMEM = " (6)";
            static char const l7[] PROGMEM = " (7)";
			static char const * const bus[] PROGMEM = {
				l0, l1, l2, l3, l4, l5, l6, l7
			};
			printf_P( pgm_read_ptr( &bus[ct] ) );
		}
	}
}

static bool checkStuckPinsForState( bool high )
{
	#define STUCKCOUNT 10
	
	printf_P( PSTR( "%S stuck pins test\n" ), high ? PSTR( "High" ) : PSTR( "Low" ) );

	uint8_t bitsa = 0b00000000;
	uint8_t bitsc = 0b00000000;
	uint8_t bitsd = 0b00000000;
	uint8_t bitsh = 0b00000000;

	DDRE |= 0b10000000;
	high ? ( PORTE |= 0b10000000 ) : ( PORTE &= 0b01111111 ); // PULLUPSENSE

	DDRA = 0b00000000; // Don't drive the bus lines
	DDRC = 0b00000000;
	DDRD = 0b00000000;
	DDRH &= 0b11101000; // Don't drive IORQ, MREQ, RD, or WR
	DDRK |= 0b00000111; // Drive the select and inhibit pins
	
	PORTA = 0b00000000;
	PORTC = 0b00000000;
	PORTD = 0b00000000;
	PORTH &= 0b11101000;


	// . .   .  .    .   .    .  .
	// . X   .  A3   .   A12  .  D4
	// . X   .  A5   .   A10  .  D2
	// . .   .  .    .   .    .  .
	//
	// . . . X  .    RD
	PORTK = 0b00000000;
	for( int i = 0; i < STUCKCOUNT; i++ ) { _NOP(); }
	bitsa |= PINA & 0b00101000;
	bitsc |= PINC & 0b00010100;
	bitsd |= PIND & 0b00010100;
	bitsh |= PINH & 0b00000010;


	// . .   .  .    .   .    .  .
	// X .   A2 .    A13 .    D5 .
	// . .   .  .    .   .    .  .
	// . X   .  A7   .   A8   .  D0
	//
	// X . . .  .    IORQ
	PORTK = 0b00000001;
	for( int i = 0; i < STUCKCOUNT; i++ ) { _NOP(); }
	bitsa |= PINA & 0b10000100;
	bitsc |= PINC & 0b00100001;
	bitsd |= PIND & 0b00100001;
	bitsh |= PINH & 0b00010000;


	// X .   A0 .    A15 .    D7 .
	// . .   .  .    .   .    .  .
	// . .   .  .    .   .    .  .
	// X .   A6 .    A9  .    D1 .
	//
	// . X . .  .    MREQ
	PORTK = 0b00000010;
	for( int i = 0; i < STUCKCOUNT; i++ ) { _NOP(); } //while(getchar() != 'a');
	bitsa |= PINA & 0b01000001;
	bitsc |= PINC & 0b10000010;
	bitsd |= PIND & 0b10000010;
	bitsh |= PINH & 0b00000100;


	// . X   .  A1   .   A14  .  D6
	// . .   .  .    .   .    .  .
	// X .   A4 .    A11 .    D3 .
	// . .   .  .    .   .    .  .
	//
	// . . . .  X    WR
	PORTK = 0b00000011;
	for( int i = 0; i < STUCKCOUNT; i++ ) { _NOP(); } //while(getchar() != 'a');
	bitsa |= PINA & 0b00010010;
	bitsc |= PINC & 0b01001000;
	bitsd |= PIND & 0b01001000;
	bitsh |= PINH & 0b00000001;

	DDRK &= 0b11111000; // Stop driving the select and inhibit lines
	PORTK &= 0b11111000;
	
	DDRE &= 0b01111111;
	PORTE &= 0b01111111;

	if( high )
	{
		bitsa ^= 0b11111111;
		bitsc ^= 0b11111111;
		bitsd ^= 0b11111111;
		bitsh ^= 0b00010111;
	}

	printStuckPins(
	bitsd,
	bitsa,
	bitsc,
	bitsh );

	return (
	bitsd == 0 &&
	bitsa == 0 &&
	bitsc == 0 &&
	bitsh == 0 );
}

static void shiftCart( uint8_t *data )
{
	// Force a parallel load
	SPDR = 0b00000000;
	while( (SPSR & _BV(SPIF)) == 0);

	// Begin reading
	uint8_t ov = 0b11111111;
	for( uint8_t i = 0; i < 4; i++ )
	{
		if( i == 3 )
		{
			ov &= 0b11111110;
		}
		SPDR = ov;
		while( (SPSR & _BV(SPIF)) == 0 );
		*(data++) = SPDR;
	}
}

static bool cartStatus( uint8_t *data )
{
	static char const l0[] PROGMEM = "imposible1";
	static char const l1[] PROGMEM = "impossible2";
	static char const l2[] PROGMEM = "D2";
	static char const l3[] PROGMEM = "D1";
	static char const l4[] PROGMEM = "D0";
	static char const l5[] PROGMEM = "A0";
	static char const l6[] PROGMEM = "A1";
	static char const l7[] PROGMEM = "A2";
	static char const l8[] PROGMEM = "A10";
	static char const l9[] PROGMEM = "A11";
	static char const l10[] PROGMEM = "D7";
	static char const l11[] PROGMEM = "D6";
	static char const l12[] PROGMEM = "D5";
	static char const l13[] PROGMEM = "D4";
	static char const l14[] PROGMEM = "D3";
	static char const l15[] PROGMEM = "C000";
	static char const l16[] PROGMEM = "shield";
	static char const l17[] PROGMEM = "A3";
	static char const l18[] PROGMEM = "A4";
	static char const l19[] PROGMEM = "A13";
	static char const l20[] PROGMEM = "A5";
	static char const l21[] PROGMEM = "A6";
	static char const l22[] PROGMEM = "A7";
	static char const l23[] PROGMEM = "E000";
	static char const l24[] PROGMEM = "GND";
	static char const l25[] PROGMEM = "VCC";
	static char const l26[] PROGMEM = "A8";
	static char const l27[] PROGMEM = "A9";
	static char const l28[] PROGMEM = "A12";
	static char const l29[] PROGMEM = "A000";
	static char const l30[] PROGMEM = "A14";
	static char const l31[] PROGMEM = "8000";
	static char const * const PROGMEM lines[] = {
		l0, l1, l2, l3, l4, l5, l6, l7, l8, l9,
		l10, l11, l12, l13, l14, l15, l16, l17, l18, l19,
		l20, l21, l22, l23, l24, l25, l26, l27, l28, l29,
		l30, l31
	};

	bool res = true;

	char const * const *lidx = lines;
	for( uint8_t i = 0; i < 4; i++ )
	{
		uint8_t v = *(data++);
		for( uint8_t j = 0; j < 8; j++ )
		{
			if( (v & 0b10000000) != 0 )
			{
				printf_P( PSTR( "%S " ), pgm_read_ptr( lidx ) );
				res = false;
			}
			v <<= 1;
			lidx++;
		}
	}
	
	if( res )
	{
		printf_P( PSTR( "OK" ) );
	}
	
	printf_P( PSTR( "\n" ) );
	
	return res;
}

static bool checkCartridgePort( )
{
	// Shift-in order:
	// 0, 0, D2, D1, D0, A0, A1, A2
	// A10, A11, D7, D6, D5, D4, D3, C000 select
	// Shield, A3, A4, A13, A5, A6, A7, E000 select
	// GND, VCC, A8, A9, A12, A000 select, A14, 8000 select

	printf_P( PSTR( "Checking cartridge port signals\n") );
	
	// Set up SPI for cartridge reader
	SPCR = _BV(SPE) | _BV(MSTR) | _BV(SPR1) | _BV(SPR0);
	SPSR = 0b00000000;
	DDRB |= 0b00000110;
	
	// Set address and data for output
	DDRA = 0b11111111;
	DDRC = 0b11111111;
	DDRD = 0b11111111;
	
	// Set memory read
	DDRH |= 0b00010111;
	PORTH = (PORTH & 0b11111001) | 0b00010001;
	
	PORTA = 0x00;
	PORTC = 0x00;
	PORTD = 0x00;
	
	// Test all cartridge lines
	printf_P( PSTR( "All address and data lines 0: " ) );
	uint8_t data[4];
	shiftCart( data );
	
	// We're only expecting the power bit and all CE bits
	data[1] ^= 0b00000001;
	data[2] ^= 0b00000001;
	data[3] ^= 0b01000101;
	bool result = cartStatus( data );
	
    printf_P( PSTR( "Address 0x8000: " ) );
	PORTC = 0x80;
	shiftCart( data );

	// Bits that should be set:  PWR, A000, C000, E000
	data[1] ^= 0b00000001;
	data[2] ^= 0b00000001;
	data[3] ^= 0b01000100;
	result = cartStatus( data ) && result;
	
    printf_P( PSTR( "Address 0xA000: " ) );
	PORTC = 0xA0;
	shiftCart( data );
	
	// Bits that should be set:  PWR, A13, 8000, C000, E000
	data[1] ^= 0b00000001;
	data[2] ^= 0b00010001;
	data[3] ^= 0b01000001;
	result = cartStatus( data ) && result;
	
    printf_P( PSTR( "Address 0xC000: " ) );
	PORTC = 0xC0;
	shiftCart( data );

	// Bits that should be set:  PWR, A14, 8000, A000, E000
	data[2] ^= 0b00000001;
	data[3] ^= 0b01000111;
	result = cartStatus( data ) && result;
	
    printf_P( PSTR( "Address 0xE000: " ) );
	PORTC = 0xE0;
	shiftCart( data );

	// Bits that should be set:  PWR, A14, A13, 8000, A000, C000
	data[1] ^= 0b00000001;
	data[2] ^= 0b00010000;
	data[3] ^= 0b01000111;
	result = cartStatus( data ) && result;

    printf_P( PSTR( "All address and data lines 1: " ) );
	PORTA = 0xff;
	PORTC = 0x3f;
	PORTD = 0xff;
	shiftCart( data );

	// Bits that should be set:  PWR, A12-A0, D7-D0, 8000, A000, C000, E000
	data[0] ^= 0b00111111;
	data[1] ^= 0b11111111;
	data[2] ^= 0b01111111;
	data[3] ^= 0b01111101;
	result = cartStatus( data ) && result;

	if( !result )
	{
		printf_P( PSTR( "\nlines are incorrect.  If any select lines (x000) are incorrect, check U5.  Otherwise check the cartridge socket pins.\n" ) );
	}
	
	DDRA = 0b00000000;
	DDRC = 0b00000000;
	DDRD = 0b00000000;
	
	DDRH &= 0b11110000;

	DDRB &= 0b11111001;
	SPCR = 0b00000000;

	return result;
}

static bool doBusTest( )
{
	printf_P( PSTR( "Checking bus lines\n" ) );
	doBusAck();
	
	bool result =
	checkStuckPinsForState( true ) &&
	checkStuckPinsForState( false ) &&
	checkCartridgePort( );

	if( result ) {
		printf_P( PSTR( "Bus test pass\n" ) );
	}
	
	return result;
}

static bool doRestingStateTest() {
	bool pass = true;

	printf_P( PSTR( "Checking resting state\n" ) );

	// Make sure all lines are at the correct resting state.
	uint8_t s = PINB;
	if( ( s & 0b10000000 ) == 0 ) { // AD4000
		printf_P( PSTR( "AD4000 is stuck active\n" ) );
		pass = false;
	}

	if( ( s & 0b01000000 ) == 0 ) { // AD2000
		printf_P( PSTR( "AD2000 is stuck active\n" ) );
		pass = false;
	}

	if( ( s & 0b00100000 ) == 1 ) { // QUAD
		printf_P( PSTR( "QUAD is stuck active\n" ) );
		pass = false;
	}

	uint8_t s2 = PINE;
	if( ( s2 & 0b00000100 ) == 0 ) { // INT
		printf_P( PSTR( "INT is stuck active\n" ) );
		if( ( s & 0b00001000 ) == 0 ) {
			printf_P( PSTR( "QUAD is ok.  This suggests a problem with U7\n" ) );
		}
		pass = false;
	}

	if( ( s2 & 0b00001000 ) == 0 ) // NMI
	{
		printf_P( PSTR( "NMI is stuck active\n" ) );
		pass = false;
	}

	if( pass ) {
		printf_P( PSTR( "Resting state looks okay\n" ) );
	}

	return pass;
}

static bool doWaitStateTest() {
	printf_P( PSTR( "Checking wait state\n" ) );
	
	// Activate M1 to see if the WAIT state begins to oscillate
	// First make sure wait state is low
	//
	// Note that the Z80 does NOT give up control over M1.  So this is a forceful pull
	// and should be finished ASAP to avoid damage to the CPU.
	DDRJ |= 0b00000010; // M1
	PORTJ |= 0b00000010;

	_NOP();
	_NOP();
	_NOP();
	_NOP();

	if( ( PING & 0b00100000 ) == 0 ) { // WAIT
		printf_P( PSTR( "WAIT line is held low\n" ) );
		return false;
	}

	PORTJ &= 0b11111101; // Activate M1

	// Grab a bunch of samples.
	uint8_t samples[SAMPLE_CNT];
	for( uint8_t i = 0; i < SAMPLE_CNT; i++ )
	{
		samples[i] = PING;
	}

	// Took the samples.  Stop pulling M1
	DDRJ &= 0b11111101;
	PORTJ &= 0b11111101;

	for( uint8_t i = 0; i < SAMPLE_CNT; i++ )
	{
		samples[i] &= 0b00100000; // WAIT
	}
	
	int waith = 0;
	int waitl = 0;
	for( uint8_t i = 1; i < SAMPLE_CNT; i++ )
	{
		samples[i] ? waith++ : waitl++;
	}

	printf_P( PSTR( "WAIT: %d:%d\n" ), waith, waitl
	);

	printf_P( PSTR( "WAIT " ) );
	bool pass = true;
	if( waith > 20 && waitl > 20 )
	{
		printf_P( PSTR( "seems ok\n" ) );
	}
	else
	{
		printf_P( PSTR( "fail\n" ) );
		pass = false;
	}

	return pass;
}

bool doU5Test() {
	printf_P( PSTR( "Checking U5\n" ) );

	bool pass = true;

	// Test basic functionality of the memory selector (U5)
	DDRA = 0b11111111;
	PORTA = 0x00;
	DDRC = 0b11111111;
	PORTC = 0x20; // Set address in 0x2000 range so that AD2000 will be tested

	DDRJ |= 0b00001000; // MEMMAPOVR
	DDRE |= 0b00010000; // RFSH.  Note that the CPU holds this line under its control, so stop fighting with it ASAP
	DDRH |= 0b00000100; // MREQ

	// With memory override active, no AD2000 under any circumstance.  Try all 4
	// combinations
	PORTJ &= 0b11110111; // MEMMAPOVR is active
	PORTE &= 0b11101111; // RFSH is active
	PORTH &= 0b11111011; // enable MREQ
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	if( ( PINB & 0b01000000 ) == 0 ) { // AD2000 line should be inactive
		printf_P( PSTR( "U5 failed override RFSH quiesce test\n" ) );
		pass = false;
	}

	PORTH |= 0b00000100; // disable MREQ
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	if( ( PINB & 0b01000000 ) == 0 ) { // AD2000 should still be inactive
		printf_P( PSTR( "U5 failed override #MREQ test\n" ) );
		pass = false;
	}

	PORTE |= 0b00010000; // Disable RFSH
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	if( ( PINB & 0b01000000 ) == 0 ) { // AD2000 should still be inactive
		printf_P( PSTR( "U5 failed override #RFSH test\n" ) );
		pass = false;
	}

	PORTH &= 0b11111011; // enable MREQ
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	if( ( PINB & 0b01000000 ) == 0 ) { // AD2000 should still be inactive
		printf_P( PSTR( "U5 failed override MREQ test.  Check U7\n") );
		pass = false;
	}

	// Allow memory mapping.
	PORTJ |= 0b00001000; // Deactivate MEMMAPOVR

	PORTE &= 0b11101111; // RFSH is active
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	if( ( PINB & 0b01000000 ) == 0 ) { // AD2000 should still be inactive
		printf_P( PSTR( "U5 failed mapped RFSH test\n" ) );
		pass = false;
	}

	PORTH |= 0b00000100; // disable MREQ
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	if( ( PINB & 0b01000000 ) == 0 ) { // AD2000 should still be inactive
		printf_P( PSTR( "U5 failed mapped #MREQ test\n" ) );
		pass = false;
	}

	PORTE |= 0b00010000; // Disable RFSH
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	if( ( PINB & 0b01000000 ) == 0 ) { // AD2000 should still be inactive
		printf_P( PSTR( "U5 failed mapped #RFSH test\n" ) );
		pass = false;
	}

	// Finally all conditions are met that AD2000 should be selected
	PORTH &= 0b11111011; // enable MREQ
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	if( ( PINB & 0b01000000 ) != 0 ) { // AD2000 should finally be active
		printf_P( PSTR( "U5 failed mapped decode AD2000 test\n" ) );
		pass = false;
	}


	// See if AD4000 goes active
	PORTC = 0x40; // Address is 0x4000 now
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	_NOP();
	if( ( PINB & 0b10000000 ) != 0 ) {
		printf_P( PSTR( "U5 failed mapped decode AD4000 test\n" ) );
		pass = false;
	}

	if( pass ) {
		printf_P( PSTR( "U5 pass\n" ) );
	}
	else
	{
		printf_P( PSTR( "U5 fail\n" ) );
	}

	// Return ports to input
	DDRA = 0b00000000;
	DDRC = 0b00000000;
	DDRJ &= 0b11110111;
	DDRE &= 0b11101111;
	DDRH &= 0b11111011;

	PORTA = 0b00000000;
	PORTC = 0b00000000;
	PORTJ &= 0b11110111;
	PORTE &= 0b00010000;
	PORTH &= 0b11111011;
	
	return pass;
}

void doBasicTests()
{
	// Hold the VDC in reset since we want to test the NMI line
	DDRJ |= 0b01000000;
	PORTJ &= 0b10111111;

    bool pass = (
	doClockTest() &&
    doResetTest() &&
    doBusAck() &&
    doBusTest() &&
    doRestingStateTest() &&
    doWaitStateTest() &&
    doU5Test());
	printf_P( PSTR( "Basic tests %S\n" ), pass ? PSTR( "passed" ) : PSTR( "failed" ) );

	// Release reset on the VDC
	DDRJ &= 0b10111111;
	PORTJ &= 0b10111111;
}
